﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
using System.Runtime.InteropServices;

namespace uam_fps_game.Physics
{
    internal struct RigidBodyProperties
    {
        internal float Mass;
        internal Vector3 CenterOfMass;
        internal Matrix InertiaTensor;

        const float EPS = 0.0001f;

        [StructLayout(LayoutKind.Sequential)]
        private struct VertexStructure
        {
            internal Vector3 Position;
            Vector3 Normal;
            Vector2 TexCoord;
            Vector3 Tangent;
            Vector3 Binormal;
        }

        static internal void CalculateFromConvexTriangleMesh(Model model, out RigidBodyProperties props)
        {
            float mass = 0.0f;
            float cx = 0.0f, cy = 0.0f, cz = 0.0f;
            float xx = 0.0f, yy = 0.0f, zz = 0.0f, yx = 0.0f, zx = 0.0f, zy = 0.0f;

            Matrix[] boneTransforms = new Matrix[model.Bones.Count];
            model.CopyAbsoluteBoneTransformsTo(boneTransforms);

            //model.Root.Transform = Matrix.Identity;
            foreach (ModelMesh mesh in model.Meshes)
            {
                foreach (ModelMeshPart part in mesh.MeshParts)
                {
                    // could only make copy of portion of buffers relevant to current part
                    short[] indices = new short[part.IndexBuffer.IndexCount];
                    part.IndexBuffer.GetData(indices);

                    VertexStructure[] vertices = new VertexStructure[part.VertexBuffer.VertexCount];
                    part.VertexBuffer.GetData(vertices);                    

                    for (int i = 0; i < part.PrimitiveCount; i++)
                    {
                        // Order of vertices switched due to winding - without it, mass and inertia params are all negative.
                        // It would probably give the same result just to negate them again instead of changing order of vertices.
                        Vector3 p1 = vertices[indices[part.StartIndex + i * 3 + 0]].Position;
                        Vector3 p3 = vertices[indices[part.StartIndex + i * 3 + 1]].Position;
                        Vector3 p2 = vertices[indices[part.StartIndex + i * 3 + 2]].Position;

                        Vector3.Transform(ref p1, ref boneTransforms[mesh.ParentBone.Index], out p1);
                        Vector3.Transform(ref p2, ref boneTransforms[mesh.ParentBone.Index], out p2);
                        Vector3.Transform(ref p3, ref boneTransforms[mesh.ParentBone.Index], out p3);

                        float x1 = p1.X; float y1 = p1.Y; float z1 = p1.Z;
                        float x2 = p2.X; float y2 = p2.Y; float z2 = p2.Z;
                        float x3 = p3.X; float y3 = p3.Y; float z3 = p3.Z;


                        // Signed volume of this tetrahedron.
                        float v = x1 * y2 * z3 + y1 * z2 * x3 + x2 * y3 * z1 -
                                 (x3 * y2 * z1 + x2 * y1 * z3 + y3 * z2 * x1);

                        // Contribution to the mass
                        mass += v;

                        // Contribution to the centroid
                        float x4 = x1 + x2 + x3; cx += (v * x4);
                        float y4 = y1 + y2 + y3; cy += (v * y4);
                        float z4 = z1 + z2 + z3; cz += (v * z4);

                        // Contribution to moment of inertia monomials
                        xx += v * (x1 * x1 + x2 * x2 + x3 * x3 + x4 * x4);
                        yy += v * (y1 * y1 + y2 * y2 + y3 * y3 + y4 * y4);
                        zz += v * (z1 * z1 + z2 * z2 + z3 * z3 + z4 * z4);
                        yx += v * (y1 * x1 + y2 * x2 + y3 * x3 + y4 * x4);
                        zx += v * (z1 * x1 + z2 * x2 + z3 * x3 + z4 * x4);
                        zy += v * (z1 * y1 + z2 * y2 + z3 * y3 + z4 * y4);  
                    }
                }
            }

            // Centroid.  
            // The case mass = 0 needs to be addressed here.
            float r = 1.0f / (4.0f * mass);

            cx *= r;
            cy *= r;
            cz *= r;

            // Mass
            mass = mass / 6.0f;

            // Moment of inertia about the centroid.
            r = 1.0f / 120.0f;
            yx = yx * r - mass * cy * cx;
            zx = zx * r - mass * cz * cx;
            zy = zy * r - mass * cz * cy;

            xx = xx * r - mass * cx * cx;
            yy = yy * r - mass * cy * cy;
            zz = zz * r - mass * cz * cz;

            props.Mass = mass;

            if (Math.Abs(cx) < EPS) cx = 0.0f;
            if (Math.Abs(cy) < EPS) cy = 0.0f;
            if (Math.Abs(cz) < EPS) cz = 0.0f;

            props.CenterOfMass = new Vector3(cx, cy, cz);

            Matrix inertiaTensor = Matrix.Identity;

            inertiaTensor.M11 = yy + zz;
            inertiaTensor.M22 = zz + xx;
            inertiaTensor.M33 = xx + yy;

            if (Math.Abs(inertiaTensor.M11) < EPS) inertiaTensor.M11 = 0.0f;
            if (Math.Abs(inertiaTensor.M22) < EPS) inertiaTensor.M22 = 0.0f;
            if (Math.Abs(inertiaTensor.M33) < EPS) inertiaTensor.M33 = 0.0f;

            if (Math.Abs(yx) < EPS) yx = 0.0f;
            if (Math.Abs(zx) < EPS) zx = 0.0f;
            if (Math.Abs(zy) < EPS) zy = 0.0f;

            inertiaTensor.M21 = inertiaTensor.M12 = -yx;
            inertiaTensor.M31 = inertiaTensor.M13 = -zx;
            inertiaTensor.M23 = inertiaTensor.M32 = -zy;

            props.InertiaTensor = inertiaTensor;
        }
    }
}
